#include <QApplication>
#include <QObject>

#include <QHostAddress>

#include <QGraphicsScene>
#include <QGraphicsView>
#include <QRect>

#include <cv.h>
#include <highgui.h>

#include <sys/time.h>

#include "client.h"

//QReadWriteLock m_mutexCamera;

Client::Client()
{
    m_laserViewer = 0;
    m_cameraViewer = 0;
    m_laser.NUM_SHOTS = 0;

    //m_server_name = "localhost";
    //m_server_name = "127.0.0.1";
    //m_server_name = "129.88.254.138";
    m_server_name = "192.168.1.151";
}

Client::~Client()
{
}

void Client::run()
{
    runThreadTCPData();
    runThreadCamera();
}

void Client::display()
{
    displayController();
    displayCamera();
    displayLaser();
}

void Client::runThreadTCPData()
{
    m_socket.abort();
    m_socket.connectToHost( m_server_name, 9876 );
    m_socket.waitForConnected( 1000 );
    
    connect( &m_socket, SIGNAL(error(QAbstractSocket::SocketError)),
             this, SLOT(serverConnectionError(QAbstractSocket::SocketError)) );

    connect( &m_socket, SIGNAL(connected()), this, SLOT(serverConnected()) );
    connect( &m_socket, SIGNAL(disconnected()), this, SLOT(serverDisconnected()) );
    connect( &m_socket, SIGNAL(readyRead()), this, SLOT(dataToRead()) );

}

void Client::runThreadCamera()
{
    // Thread to get video streaming via http
    m_mjpegClient = new MJPEGClient( m_server_name );

    QObject::connect( m_mjpegClient, SIGNAL( gotNewImage( const QImage * ) ), this, SLOT( gotNewSensorData( const QImage * ) ) );

    //m_mjpegClient->resetPanTitl();
    m_mjpegClient->getVideoFromStreamer();
}

void Client::serverConnectionError( QAbstractSocket::SocketError error )
{
    if( error == QAbstractSocket::RemoteHostClosedError )
	return;

    qDebug() << "ERROR: problem of connection to Wifibot " << m_socket.errorString();

    QApplication::instance()->quit();
}

void Client::serverConnected()
{
    qDebug() << "connected to Wifibot";
}

void Client::serverDisconnected()
{
    qDebug() << "disconnected from Wifibot";

    QApplication::instance()->quit();
}

void Client::dataToRead()
{
    QDataStream stream( &m_socket );
    stream.setVersion( QDataStream::Qt_4_0 );

    //qDebug() << "bytes available: " << m_socket.bytesAvailable();

    while ( ! stream.atEnd() && m_socket.bytesAvailable() >= 5500 )
    {
	//if ( pinfo == "L" )

	{
	    LaserData laserdata;
	    stream >> laserdata.MAX_RANGE;
	    stream >> laserdata.MIN_RANGE;
	    stream >> laserdata.NUM_SHOTS;
	    stream >> laserdata.START_ANGLE;
	    stream >> laserdata.END_ANGLE;
	    stream >> laserdata.RESOLUTION;

	    //qDebug() << laserdata.MAX_RANGE << laserdata.NUM_SHOTS << laserdata.RESOLUTION;

	    laserdata.dist.clear();
	    laserdata.dist.resize( laserdata.NUM_SHOTS );

	    for ( int i = 0; i < laserdata.NUM_SHOTS; i++ )
		stream >> laserdata.dist[i];

	    emit gotNewSensorData( &laserdata );
	}
    }
}

void Client::dataToSend( int cmd_left, int cmd_right )
{
    QDataStream stream( &m_socket );
    stream.setVersion( QDataStream::Qt_4_0 );

    stream << (quint16)cmd_left << (quint16)cmd_right;

    m_socket.flush();
    m_socket.waitForBytesWritten();
}

void Client::gotNewSensorData( const RS232Data* sensorL, const RS232Data* sensorR )
{
    unsigned long int timeStamp = getCurrentTime();
    qDebug() << "Odometry timestamp : " << timeStamp << "Speed left: " << sensorL->speed_front << "Speed right: " << sensorR->speed_front;
}

void Client::gotNewSensorData( const QImage* pic )
{
    //m_mutexCamera.lockForRead();

    static bool connected = false;

    if ( !connected )
    {
	connected = true;
	qDebug() << "camera connected";
    }

    m_camera.m_timeStamp = getCurrentTime();

    //qDebug() << "Camera time stamp: " << m_camera.m_timeStamp;

    m_camera.m_pic = *pic;

    //m_mutexCamera.unlock();

    exampleImageProcessing();
}

void Client::gotNewSensorData( LaserData* laserdata )
{
    static bool laserConnected = false;

    if ( ! laserConnected )
    {
	laserConnected = true;
	qDebug() << "laser connected";
    }

    m_laser.m_timeStamp = getCurrentTime();

    m_laser.NUM_SHOTS = laserdata->NUM_SHOTS;
    m_laser.MAX_RANGE = laserdata->MAX_RANGE;
    m_laser.START_ANGLE = laserdata->START_ANGLE;
    m_laser.END_ANGLE = laserdata->END_ANGLE;
    m_laser.RESOLUTION = laserdata->RESOLUTION;

    if ( m_laser.dist.size() != m_laser.NUM_SHOTS )
    {
	m_laser.dist.clear();
	m_laser.dist.resize( m_laser.NUM_SHOTS );
    }

    for ( int i = 0; i < m_laser.NUM_SHOTS; i++ )
	m_laser.dist[i] = laserdata->dist[i];

    if ( m_laserViewer )
    {
	m_laserViewer->update( m_laserViewer->boundingRect() );
    }
}

// example of image processing using OpenCV library
void Client::exampleImageProcessing()
{
    // get the camara image's size
    int width = m_camera.m_pic.width();
    int height = m_camera.m_pic.height();

    // convert QImage format to OpenCV format for processing
    IplImage* pic_cv = qImage2IplImage( &m_camera.m_pic );

    // convert to grayscale image
    IplImage* pic_gray = cvCreateImage( cvSize( width, height ), IPL_DEPTH_8U, 1 );
    cvCvtColor( pic_cv, pic_gray, CV_RGB2GRAY );

    // apply Canny edge detection
    IplImage* pic_edge = cvCreateImage( cvSize( width, height ), IPL_DEPTH_8U, 1 );
    cvCanny( pic_gray, pic_edge, 10, 100, 3 );

    // display the processed image with OpenCV
    cvNamedWindow( "Edge detection", CV_WINDOW_AUTOSIZE );
    cvShowImage( "Edge detection", pic_edge );

    cvReleaseImage( &pic_cv );
    cvReleaseImage( &pic_gray );
    cvReleaseImage( &pic_edge );
}

void Client::exampleLidarProcessing()
{
}

void Client::exampleRobotControlling()
{

}

void Client::exampleCameraControlling()
{
    //m_mjpegClient->resetPanTitl();
    //m_mjpegClient->setLookUp();
}

void Client::displayLaser()
{
    // laser data viewer
    m_scene = new QGraphicsScene( QRect( 0, 0, 500, 500 ) );

    m_laserViewer = new QiLaserScan( m_laser );
    m_laserViewer->setFlag( QGraphicsItem::ItemIsFocusable );

    m_scene->addItem( m_laserViewer );
    m_laserViewer->setPos( 250, 250 );

    //m_laserViewer->setFocus();
    m_scene->setFocusItem( m_laserViewer );

    m_view = new QGraphicsView();
    m_view->setScene( m_scene );
    m_view->show();

    m_view->scale( 1.0, -1.0 );

}

void Client::displayCamera()
{
    m_cameraViewer = new QwImageViewer( 320, 240 );

    QObject::connect( m_mjpegClient, SIGNAL( gotNewImage( const QImage * ) ), m_cameraViewer, SLOT( refresh( const QImage * ) ) );
    QObject::connect( m_cameraViewer, SIGNAL( cmdReceived(QKeyEvent*) ), m_mjpegClient, SLOT( cmdProcessing(QKeyEvent*) ) );

    m_cameraViewer->show();
}

void Client::displayController()
{
    m_controllerViewer = new QwController();

    connect( m_controllerViewer, SIGNAL( newCommand(int, int) ), this, SLOT( dataToSend(int, int) ) );

    m_controllerViewer->show();
}


unsigned long int getCurrentTime()
{
    struct timeval time_d;

    gettimeofday( &time_d, NULL );

    return time_d.tv_sec * 1000000 + time_d.tv_usec;

}


